#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "gps.h"
#include "gps_serial/targetGPS.h"
#include "sensor_msgs/NavSatFix.h"
#include "string.h"

//由于51单片机为Big-Endian,而STM32为Little-Endian
//故定义宏如下,以实现转换
#define BE_TO_LE_16(A)	((((uint16_t)(A) & 0xff00) >> 8) | (((uint16_t)(A) & 0x00ff) << 8))
#define BE_TO_LE_32(A)	((((uint32_t)(A) & 0xff000000) >> 24) | \
												 (((uint32_t)(A) & 0x00ff0000) >>  8) | \
												 (((uint32_t)(A) & 0x0000ff00) <<  8) | \
												 (((uint32_t)(A) & 0x000000ff) << 24))

//数据解析函数，接收缓冲区，接收数据个数，处理结果
int NMEdataAnalysis(uint8_t *buffer, size_t n, NMEA_MSG &nmea_msg);

ros::Publisher targetGPSPub; //自定义消息类型
ros::Publisher targetPosPub; //标准消息类型

int main(int argc, char** argv)
{
	ros::init(argc, argv, "gps_serial_node");
	ros::NodeHandle n;

    std::string serialPortName;
    n.param<std::string>("/gps_serial_node/gpsSerialPort", serialPortName, "/dev/ttyUSB0");

    targetGPSPub = n.advertise<gps_serial::targetGPS>("target_Position/targetGPS", 10);
    targetPosPub = n.advertise<sensor_msgs::NavSatFix>("target_Position/targetPos", 10);

    gps_serial::targetGPS gpsMsg;
    sensor_msgs::NavSatFix posMsg;

	//创建一个serial类对象
	serial::Serial sp;

    //创建timeout
	serial::Timeout to = serial::Timeout::simpleTimeout(100);
	sp.setPort(serialPortName);
	sp.setBaudrate(115200);
	sp.setTimeout(to);

	try
	{
        //打开串口
		sp.open();
	}
	catch(serial::IOException& e)
	{
		ROS_ERROR("Unable to open port.");
		return -1;
	}

    //判断串口是否打开成功
	if(sp.isOpen())
	{
		ROS_INFO("%s is opened.", serialPortName.c_str());
	}
	else
	{
		return -1;
	}

	ros::Rate loop_rate(100);//由于数据接收频率最大20，所以100足以应对
	while(ros::ok())
	{
        //获取缓冲区内的字节数
		size_t n = sp.available();
		if(n != 0)
		{
			uint8_t buffer[1024];
            NMEA_MSG nmea_msg;//定义变量存储接收到的原始数据
            
            //读数据，此时n被重新赋值为读取到的字节数
			n = sp.read(buffer, n);
            ros::Time Time = ros::Time::now();//当前时间

            if(0 == NMEdataAnalysis(buffer, n, nmea_msg))
            {
                gpsMsg.header.stamp = Time;
                gpsMsg.header.frame_id = "Target GPS";

                gpsMsg.hour = nmea_msg.utc.hour;
                gpsMsg.minute = nmea_msg.utc.min;
                gpsMsg.second = nmea_msg.utc.sec;

                gpsMsg.latitude = (BE_TO_LE_32(nmea_msg.latitude))/100000.0;
                gpsMsg.longitude = (BE_TO_LE_32(nmea_msg.longitude))/100000.0;
                gpsMsg.ns = nmea_msg.nshemi;
                gpsMsg.ew = nmea_msg.ewhemi;

                gpsMsg.hdop = (BE_TO_LE_16(nmea_msg.hdop))/10.0;

                float altitudeTemp = (BE_TO_LE_16(nmea_msg.altitude.value))/10.0;
                gpsMsg.altitude = (nmea_msg.altitude.sign == 0) ? altitudeTemp : -altitudeTemp;
                
                gpsMsg.gpsStatus = nmea_msg.gpssta;
                gpsMsg.posslnum = nmea_msg.posslnum;

                targetGPSPub.publish(gpsMsg);//发布自定义消息
                std::cout << gpsMsg << std::endl;

                posMsg.header = gpsMsg.header;//header格式都一样
                posMsg.latitude = gpsMsg.latitude;
                posMsg.longitude = gpsMsg.longitude;
                posMsg.altitude = gpsMsg.altitude;
                targetPosPub.publish(posMsg);//发布标准格式消息
            }
		}
		loop_rate.sleep();
	}
	
	sp.close(); //关闭串口
	return 0;
}

//数据解析函数定义
int NMEdataAnalysis(uint8_t *buffer, size_t n, NMEA_MSG &nmea_msg)
{
    size_t index;
    size_t returnValue = -1;//默认返回值代表没有找到定界符
    for(index = 0; index < n; index++)
    {
        if(buffer[index] == 0x7e)
        {//查找帧定界符0xfe
            if((index++ < n - 24) && (buffer[index++] == 0x7d) && (buffer[index++] == 0x20) && (buffer[index++] == 0x14))
            {//查看连续的几个字符是否符合规定
                uint8_t dataBuf[21]; //存储去除转义字符的数据及校验和
                int count = 0; //标记当前去除转义字符后的字节总数
                uint8_t checkSum = 0; //校验和累加变量
                while(count < 21)
                {
                    if(buffer[index] != 0x7d)
                    {//如果待处理字节不是是转义字符
                        dataBuf[count] = buffer[index];
                    }
                    else
                    {//否则跳过转义字符，处理其下一个字节
                        if(++index < n)
                        {//只要index增加就要检查数组是否越界
                            dataBuf[count] = buffer[index]^0x20;
                        }
                        else
                        {//如果越界则直接退出while循环
                            break;
                        }
                    }
                    checkSum += dataBuf[count]; //将转义后的字节累加
                    if(++index >= n)
                    {//index增加，指向下一个字节
                        break;
                    }
                    count++;//count增加，指向下一个字节
                }

                if((count == 21) && (checkSum == 0xff))
                {//数据完整且校验通过
                    // for(int i = 0; i < 21; i++)
                    // { //按16进制显示字符
                    // 	std::cout << std::hex << (dataBuf[i] & 0xff) << " ";
                    // }
                    // std::cout << std::endl;

                    unsigned char *nmea_msg_p = (unsigned char *)&nmea_msg;
                    for(int i = 0; i < 20; i++)
                    {
                        *(nmea_msg_p + i) = dataBuf[i];
                    }

                    returnValue = 0;//返回值为0代表成功
                }
                else
                {
                    returnValue = -3;//返回值为-3代表数据接收不完整或数据区错误
                }
                
            }
            else
            {
                returnValue = -2;//返回值为-2代表数据接收不完整或帧头错误
            }

            break;//只要找到定界符，无论后续数据如何，都退出此轮循环
        }
    }

    return returnValue;
}